#include <ros/ros.h>
#include <pr2_hri/HriAction.h>
#include "left_arm.h"
#include "right_arm.h"
#include <actionlib/server/simple_action_server.h>
#include "sensor_msgs/JointState.h"

const static unsigned int INDEX_OF_HEAD_PAN = 14;
static float headPan = 0.0f;

class HriAction
{
private:
  LeftArm leftArm;
  RightArm rightArm;

  void raise_camera_to_eye() {
    ROS_INFO("raising camera to eye");
    // Start the trajectory
    leftArm.startTrajectory(leftArm.cameraToEyeTrajectory(headPan));
  }

  void track_eye() {
    ROS_INFO("tracking eye");
    // Start the trajectory
    leftArm.startTrajectory(leftArm.cameraToEyeTrajectory(headPan, 2.5f));
  }

  void lower_camera_from_eye() {
    ROS_INFO("lowering camera from eye");
    leftArm.startTrajectory(leftArm.cameraFromEyeTrajectory());
  }

  void move_in() {
    ROS_INFO("gesturing \"move in\"");
    rightArm.startTrajectory(rightArm.moveInTrajectory(headPan));
  }

  void move_left() {
    ROS_INFO("gesturing \"move left\"");
    rightArm.startTrajectory(rightArm.moveLeftTrajectory(headPan));
  }

  void move_right() {
    ROS_INFO("gesturing \"move right\"");
    rightArm.startTrajectory(rightArm.moveRightTrajectory(headPan));
  }

  void right_arm_down() {
    ROS_INFO("gesturing \"right arm down\"");
    rightArm.startTrajectory(rightArm.armDownTrajectory());
  }

protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<pr2_hri::HriAction> as_;
  std::string action_name_;
  // create messages that are used to published feedback/result
  pr2_hri::HriFeedback feedback_;
  pr2_hri::HriResult result_;

public:

  HriAction(std::string name) :
    as_(nh_, name, boost::bind(&HriAction::executeCB, this, _1), false),
    action_name_(name)
  {
    ROS_INFO("HriAction()");
    as_.start();
  }

  ~HriAction(void) {
    ROS_INFO("~HriAction()");
  }

  void executeCB(const pr2_hri::HriGoalConstPtr &goal) {
    
    // helper variables
    ros::Rate r(1);
    bool success = true;
    ROS_INFO("Goal: %s", goal->goalString.data.c_str());
    if (goal->goalString.data == "raise_camera_to_eye") {
      raise_camera_to_eye();
      success = leftArm.waitForResult(30.0);
    } else if (goal->goalString.data == "track_eye") {
      track_eye();
      success = leftArm.waitForResult(30.0);
    } else if (goal->goalString.data == "lower_camera_from_eye") {
      lower_camera_from_eye();
      success = leftArm.waitForResult(30.0);
    } else if (goal->goalString.data == "move_in") {
      ROS_INFO("move-in starting");
      move_in();
      success = rightArm.waitForResult(30.0);
      ROS_INFO("move-in finished");
    } else if (goal->goalString.data == "move_left") {
      ROS_INFO("move-left starting");
      move_left();
      success = rightArm.waitForResult(30.0);
      ROS_INFO("move-left finished");
    } else if (goal->goalString.data == "move_right") {
      ROS_INFO("move-right starting");
      move_right();
      success = rightArm.waitForResult(30.0);
      ROS_INFO("move-right finished");
    } else if (goal->goalString.data == "right_arm_down") {
      ROS_INFO("right-arm-down starting");
      right_arm_down();
      success = rightArm.waitForResult(30.0);
      ROS_INFO("right-arm-down finished");
    }


    std_msgs::String s;
    s.data = "result_.data";
    result_.resultString = s;

    if(success)
    {
      //result_.sequence = feedback_.sequence;
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }

};

void trajCallback(const sensor_msgs::JointState::ConstPtr& msg) {
  //ROS_INFO("I heard [%s]", msg->name[INDEX_OF_HEAD_PAN].c_str());
  //ROS_INFO("I position [%f]", msg->position[INDEX_OF_HEAD_PAN]);
  headPan = msg->position[INDEX_OF_HEAD_PAN];
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "HriActionServer");

  HriAction hri(ros::this_node::getName());
  ros::NodeHandle n;
  ros::Subscriber traj_subscriber =
      n.subscribe("joint_states", 1000, trajCallback);
  ros::spin();

  return 0;
}
